clc; clear
 
 A =[0 0 
     0 30 
     30 30 
     30 0
     0 0
] ;
 B =[11.149906	18.121528
-3.120794	9.912528
];
 C =[12.777706	15.037528
-1.492994	6.828528
];
D=[1.380606	2.756528
15.301006	10.696528
14.405506	11.953528
0.134806	3.744528
1.380606	2.75652853
];
 
figure(3);
plot(A(:,1),A(:,2));hold on;
% plot(B(:,1),B(:,2),':r');hold on;
% plot(C(:,1),C(:,2),':g');hold on;
% plot(D(:,1),D(:,2),':k');hold on;
%6.53023	3.970426









 
%% 前轮偏转角度函数 fdelta 和后轴中点速度函数 fv 可由控制函数或手动设置得到
% % [fdelta, fv] = autocontrol(bd, amax, vmax, omega, phimax, dir); 
% 前轮最大转动角速度为 max(dbeta) = 400/16 = 25 deg/s，这里取为 20 deg/s
dbeta = 25;   % [deg/s] 前轮转动角速度，可增大至 25
maxalf = 470/16;
tmax = 470/16/dbeta; % 前轮最大偏转角为 470/16 度，则转动时间为 470/16/dbeta
zalog = 90;% 车身初始角度

re= 180;% 墙布角度
vec = 0.5;% 普通速度
l=2.8;% 车身长
alpa = (re-zalog);
 womiga = rad2deg(vec*tand(maxalf)/l);
%  womiga = vec*(maxalf)/l;

t2 = (alpa-6.278486626882780)/womiga;

the_max_error = 6.2785;
ala=5;
wegalf = deg2rad(ala);
dbeta1=deg2rad(dbeta);
rotefunction =@(x)exp(-wegalf*l*dbeta1/(2*vec))-cos(dbeta1*x);
daofun = @(x) dbeta1*sin(dbeta1*x);
tort=acos(exp(-wegalf*l*dbeta1/(2*vec)))/dbeta1;
%tort = newiteration(rotefunction,daofun,0.5,0.00000001);
disp(tort);
tzero = 0;
time =adjust_car_angle(vec,25,zalog,re);
fdelta = @(t) 0 + ... 
             dbeta*min(t-0,time(1)).*(t>0) - ...
            dbeta*min(t-time(1)-time(2),time(1)).*(t>(time(1)+time(2)));
% 前轮偏转角度随时间的变化（= 盘转动角度/16）：
% fdelta = @(t) 0 + ... 
%               dbeta*min(t-0,tmax*1).*(t>0) - ...
%               dbeta*min(t-tmax-t2,tmax*1).*(t>(tmax+t2));
%              
%          fdelta = @(t) 0 + ... 
%               dbeta*min(t-0,tmax*1).*(t>0) - ...
%               dbeta*min(t-tmax,tmax*1).*(t>(tmax)); 


% 

% fdelta = @(t) 0 + ... 
%               dbeta*min(t-0,tort).*(t>0) - ...
%               dbeta*min(t-tort-tzero,tort).*(t>(tort+tzero));





% 后轴中点速度（由油门和刹车控制）
fv = @(t)min(t*2,0.5) ;

% 求解微分方程的时间间隔和范围j
dt = 0.01;
t = [0:dt:1.1750]';

% 车身角度 phi，车身中心坐标 (xc,yc) 的初始值 y0 = [phi0, xc0, yc0]
y0 = [zalog,15,5];

% 求解微分方程数值解，得到任意时刻车身的转角 phi(t) 和车身中心位置 (xc(t), yc(t))
[t,y] = ode45(@odecar, t, y0, [], fv, fdelta);
phi = y(:,1); xc = y(:,2); yc = y(:,3);

h = plotcar1(15,5,zalog,0); hold on;
h2 = plot(15,5, ':m', 'linewidth',2);



% 动态绘图
for i = 1:10:length(t)
    plotcar1(xc(i),yc(i),phi(i),fdelta(t(i)),h);
    disp(phi(i));
    set(h2, 'XData', xc(1:i),'YData', yc(1:i));
    drawnow
end

figure
subplot(1,2,1); plot(t,fdelta(t)*16); xlabel('时间(s)'); ylabel('方向盘角度')
subplot(1,2,2); plot(t,fv(t));        xlabel('时间(s)'); ylabel('后车轮速度')



